/***************************************************************************
 创建者: 李文友
 开始时间: 2018.9.10
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) 李文友 2018.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "grbladapter.h"
#include <qdebug.h>
#include <QObject>
#include <QTime>
#include <QCoreApplication>
#include <QSerialPortInfo>
extern "C"
{
    #include "serial.h"
    #include "grbl.h"
}
// Declare system global variable structure
system_t sys;


#include <stdio.h>
#include <string.h>
#include <malloc.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <math.h>

#define MAX_BUFFER_SIZE 512

#define D_USE_QSERIAL 1

int fd, s;

int open_serial()
{
    //这里的/dev/pts/27是使用mkptych.py虚拟的两个串口名字之一
    fd = open("/dev/pts/3", O_RDWR|O_NOCTTY|O_NDELAY);
    if(fd == -1)
    {
        perror("open serial port error!\n");
        return -1;
    }

    printf("open /dev/pts/21.\n");
    return 0;
}

GrblAdapter::GrblAdapter(QString serialDeviceNameIn)
{
    isInitialOkStatus=false;
    serialPortConfig = new SerialPortConfig ;
    serialPort_device = new QSerialPort;

    //serial set
    serialPortConfig->isUseUsbDeviceSerialCode=0;
    serialPortConfig->serialDeviceName=serialDeviceNameIn;
    serialPortConfig->baudRate=QSerialPort::Baud9600;         //　波特率
    serialPortConfig->dataBits=QSerialPort::Data8;         //  数据位
    serialPortConfig->parity=QSerialPort::NoParity;   //  校验位
    serialPortConfig->stopBits=QSerialPort::OneStop;         //  停止位

    if(D_USE_QSERIAL)
    {
        // 初始化串口
        int tmpKey=initalSerialPort();
        if(1==tmpKey)
        {
            isInitialOkStatus = true;
        }


        if(serialPort_device->isOpen())
        {
            serialPort_device->setDataTerminalReady(true);
    //    timer.start( serialPortConfig->updatePeriod );
            timer.start( 100);
        }
    //    QObject::connect(&timer, SIGNAL( timeout() ), this, SLOT(timeOut_slot()));
        QObject::connect(serialPort_device, SIGNAL(readyRead() ), this, SLOT(receiveData()));

    }

//    createThread();
//    createThread2();

}


/**
 * @brief GrblAdapter::receiveData  接收数据
 * @return  success:1   failed:-1
 */
int GrblAdapter::receiveData()
{
    //数据预判断
    int bufferSize=serialPort_device->bytesAvailable();
    QByteArray receiveBatteryData;
    if(1<=bufferSize)
    {
        receiveBatteryData = serialPort_device->readAll();
        qDebug() << "sucess：GrblAdapter::receiveData() dataIn" << receiveBatteryData.toHex();
        for(int i=0;i<receiveBatteryData.size();i++)
        {
            setReceive_ISR(receiveBatteryData[i]);
        }

        return 1;
    }
    else
    {
        return 0;
    }


    return 1;
}


/**
 * @brief GrblAdapter::initalSerialPort  串口配置初始化
 * @return      success:1  failed:-1
 */
int GrblAdapter::initalSerialPort()
{
    bool isFinded=false;
    /**
     * @brief serialPortInfo            串口信息对象
     */
    QSerialPortInfo serialPortInfo;
    foreach ( serialPortInfo, serialPortInfo.availablePorts() )
    {
        qDebug() << "Name : " << serialPortInfo.portName();
        qDebug() << "Description : " << serialPortInfo.description();
        qDebug() << "serialNumber: " << serialPortInfo.serialNumber();
        qDebug() << "productIdentifier:" << serialPortInfo.productIdentifier();
        if(1==serialPortConfig->isUseUsbDeviceSerialCode)
        {
            if( serialPortInfo.serialNumber() == serialPortConfig->serialDeviceName)
            {
                // 设置端口号
                serialPort_device->setPortName(serialPortInfo.portName());
                qDebug()<<"find COM"<<serialPortInfo.portName();
                isFinded=true;
                break;
            }
        }
        else
        {
            if( serialPortInfo.portName() == serialPortConfig->serialDeviceName )
            {
                // 设置端口号
                serialPort_device->setPortName(serialPortConfig->serialDeviceName);
                qDebug()<<"find COM"<<serialPortInfo.portName();
                isFinded=true;
                break;
            }
        }

    }
    // 设置端口号
//    serialPort_device->setPortName(serialPortConfig->serialDeviceName);
//    qDebug()<<"find COM"<<serialPortInfo.portName();
//    isFinded=true;
    if(false==isFinded)
    {
        return -1;
    }
    qDebug() << "serilaPortName:" << serialPortInfo.portName();

    if( serialPort_device->open(QIODevice::ReadWrite) )
    {
       // 配置串口参数
       serialPort_device->setBaudRate(serialPortConfig->baudRate);         //　波特率
       serialPort_device->setDataBits(serialPortConfig->dataBits);         //  数据位
       serialPort_device->setParity(serialPortConfig->parity);             //  校验位
       serialPort_device->setStopBits(serialPortConfig->stopBits);         //  停止位
       serialPort_device->setFlowControl(QSerialPort::NoFlowControl);      //  流控制
       qDebug() << "串口打开成功:" << serialPort_device->portName();
   }
   else
    {
       qDebug() << "串口打开失败:" << serialPort_device->portName();
        return -1;
    }

    return 1;
}

//int GrblAdapter::setLaserIntensity_once(int intensityIn)
//{
//    int rawValue=4095*intensityIn/1000;
//    unsigned char hightByte=rawValue>>8;
//    unsigned char lowByte=rawValue&0xff;
//    unsigned char endFlag=0x0d;
//    QByteArray sendCommand;
//    sendCommand.append(hightByte);
//    sendCommand.append(lowByte);
//    sendCommand.append(endFlag);
//    serialPort_device->write(sendCommand);


//    qDebug()<<"serial send:"<<sendCommand;
//    //等待返回数据．
//    QTime dieTime = QTime::currentTime().addMSecs(1000);
//    while( QTime::currentTime() < dieTime )
//    {
//        QCoreApplication::processEvents(QEventLoop::AllEvents, 200);

//    }

//    return 1;
//}

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* GrblAdapter::threadRun(void *)
{
    printf("SafetyFunction theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 50;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************threadRun1 pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);


    // Initialize system upon power-up.
    serial_init();   // Setup serial baud rate and interrupts
    settings_init(); // Load Grbl settings from EEPROM
    stepper_init();  // Configure stepper pins and interrupt timers
    system_init();   // Configure pinout pins and pin-change interrupt

    memset(&sys, 0, sizeof(system_t));  // Clear all system variables
    sys.abort = true;   // Set abort to complete initialization
//    sei(); // Enable interrupts

    // Check for power-up and set system alarm if homing is enabled to force homing cycle
    // by setting Grbl's alarm state. Alarm locks out all g-code commands, including the
    // startup scripts, but allows access to settings and internal commands. Only a homing
    // cycle '$H' or kill alarm locks '$X' will disable the alarm.
    // NOTE: The startup script will run after successful completion of the homing cycle, but
    // not after disabling the alarm locks. Prevents motion startup blocks from crashing into
    // things uncontrollably. Very bad.
    #ifdef HOMING_INIT_LOCK
      if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; }
    #endif

    // Force Grbl into an ALARM state upon a power-cycle or hard reset.
    #ifdef FORCE_INITIALIZATION_ALARM
      sys.state = STATE_ALARM;
    #endif

    // Grbl initialization loop upon power-up or a system abort. For the latter, all processes
    // will return to this loop to be cleanly re-initialized.
    for(;;) {

      // TODO: Separate configure task that require interrupts to be disabled, especially upon
      // a system abort and ensuring any active interrupts are cleanly reset.

      // Reset Grbl primary systems.
      serial_reset_read_buffer(); // Clear serial read buffer
      gc_init(); // Set g-code parser to default state
      spindle_init();
      coolant_init();
      limits_init();
//      probe_init();
      plan_reset(); // Clear block buffer and planner variables
      st_reset(); // Clear stepper subsystem variables.

      // Sync cleared gcode and planner positions to current system position.
      plan_sync_position();
      gc_sync_position();

      // Reset system variables.
      sys.abort = false;
      sys_rt_exec_state = 0;
      sys_rt_exec_alarm = 0;
      sys.suspend = false;
      sys.soft_limit = false;

      // Start Grbl main loop. Processes program inputs and executes them.
      protocol_main_loop();

    }

    printf("GrblAdapter thread quit!!!");
}

void* GrblAdapter::threadTask( void* classPtr )
{
    qDebug()<<"GrblAdapter thread run-------------------------";
    return ((GrblAdapter*)classPtr)->threadRun(NULL);
}

void GrblAdapter::createThread()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &GrblAdapter::threadTask,this);
    qDebug()<<"GrblAdapter createThread-------------------------";
}


void* GrblAdapter::threadRun2(void *)
{
    printf(" theread2 start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 50;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************threadRun2 pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);


    if(D_USE_QSERIAL)
    {
        while(1)
        {

            int tmpBufferCount=serial_get_tx_buffer_count();
//            qDebug()<<"tmpBufferCount:"<<tmpBufferCount;
            if(tmpBufferCount>0)
            {
                unsigned char tmpChar;
                QByteArray sendCommand;
                for(int i=0;i<tmpBufferCount;i++)
                {
                    tmpChar=serialGetData();
                    sendCommand.append(tmpChar);
                }

                 serialPort_device->write(sendCommand);
                 qDebug()<<"send:"<<sendCommand;
                 usleep(100000);
            }

        }
    }
    else
    {
        char hd[MAX_BUFFER_SIZE], *rbuf;
        int flag_close, retv;
        struct termios opt;

        retv = open_serial();
        if(retv < 0)
        {
            printf("Open serrial port error!\n");

        }
        else
        {
            qDebug()<<"open serial sucess";
        }

        tcgetattr(fd, &opt);
        cfmakeraw(&opt);
        cfsetispeed(&opt, B9600);
        cfsetospeed(&opt, B9600);
        tcsetattr(fd, TCSANOW, &opt);
        rbuf = hd;
        printf("Ready for receiving data...\n");

        while(1)
        {
            while((retv = read(fd, rbuf, 1)) > 0)
            {
    //            printf( "receive:%d \n", *rbuf);
                setReceive_ISR(*rbuf);
                QChar tmpStr(*rbuf);
                qDebug()<<"receive:"<<tmpStr;
            }

            int tmpBufferCount=serial_get_tx_buffer_count();
            if(tmpBufferCount>0)
            {
                qDebug()<<"tmpBufferCount:"<<tmpBufferCount;
                uint8_t sbuf=serialGetData();
                int length = sizeof(sbuf);
                retv = write(fd, &sbuf, length);
                if(retv == -1)
                {
                    qDebug()<<"send data error!";
                }
                else
                {
                    QChar tmpStr(sbuf);
                    qDebug()<<"send:"<<tmpStr;
                }
            }
        }

        printf("\n");
        flag_close = close(fd);
        if(flag_close == -1)
            printf("Close the device failure!\n");

    }


    printf("GrblAdapter thread２ quit!!!");
}

void* GrblAdapter::threadTask2( void* classPtr )
{
    qDebug()<<"GrblAdapter thread2 run-------------------------";
    return ((GrblAdapter*)classPtr)->threadRun2(NULL);
}

void GrblAdapter::createThread2()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &GrblAdapter::threadTask2,this);
    qDebug()<<"GrblAdapter createThread2-------------------------";
}


void GrblAdapter::timeOut_slot()
{

}


bool GrblAdapter::isInitialOk()
{
    return isInitialOkStatus;
}
